#include "../User/Component/CMD.h"

bool CMD_PraseCommand(CMD_Send_t *cmd, uint8_t *data) {
  if (data[4] != 'l') return false;
  int8_t *tmp = (int8_t *)&data[1];
  switch (data[0]) {
    case 'p':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  10.0f;
      cmd->operation = CMD_OPERATION_ANGLE_P;
      break;
    case 'i':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  10.0f;
      cmd->operation = CMD_OPERATION_ANGLE_I;
      break;
    case 'd':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  10.0f;
      cmd->operation = CMD_OPERATION_ANGLE_D;
      break;
    case 'P':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  10.0f;
      cmd->operation = CMD_OPERATION_SPEED_P;
      break;
    case 'I':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  100.0f;
      cmd->operation = CMD_OPERATION_SPEED_I;
      break;
    case 'D':
      cmd->data = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                          (tmp[2] - '0')) /
                  100.0f;
      cmd->operation = CMD_OPERATION_SPEED_D;
      break;
    default:
      return false;
  }
  return true;
}
